#include <MapKit/MapKit.h>
#include <glm/glm.hpp>

void MapKit::Triangulate()
{
    cvec_t* cells = new cvec_t;
    vtvec_t* vtsConvexHull = nullptr;
    ConvexHull(vtsConvexHull, coords);
    vtvec_t rests;
    cvec_t standby;
    for (auto p : *coords)
    {
        bool found = false;
        for (auto q : *vtsConvexHull)
            if (p == q)
            {
                found = true;
                break;
            }
        if (!found)
            rests.push_back(p);
    }
    bool noInnerVertices = false;
TAG_SPLIT:
    if (rests.size() > 0)
    {
        auto itICHFinal = vtsConvexHull->end();
        itICHFinal--;
        for (auto it = vtsConvexHull->begin(); it != vtsConvexHull->end(); it++)
        {
            auto itNext = it;
            itNext++;
            coord_t v1 = *it;
            coord_t v2;
            if (it != itICHFinal)
                v2 = *itNext;
            else
            {
                if (noInnerVertices)
                    break;
                else
                    v2 = *(vtsConvexHull->begin());
            }
            cvec_t acpt;
            cell_t single = { *(rests.begin()), v1, v2 };
            single.Sort();
            standby.push_back(single);
            while (!standby.empty())
            {
                auto ins = *(standby.begin());
                standby.erase(standby.begin());
                bool engage = true;
                for (auto itc = cells->begin(); itc != cells->end(); itc++)
                {
                    auto old = *itc;
                    if (ins == old)
                    {
                        engage = false;
                        break;
                    }
                    coord_t va, vb, o1, o2;
                    bool common = FindCommon(va, vb, o1, o2, ins, old);
                    if (common)
                    {
                        bool inCircumcircle = IsVertexInCircumcircle(old.A, old.B, old.C, o1);
                        if (inCircumcircle)
                        {
                            cell_t c1 = { o1, o2, va };
                            c1.Sort();
                            cell_t c2 = { o1, o2, vb };
                            c2.Sort();
                            standby.push_back(c1);
                            standby.push_back(c2);
                            cells->erase(itc);
                            engage = false;
                            break;
                        }
                    }
                }
                if (engage)
                    acpt.push_back(ins);
            }
            while (!acpt.empty())
            {
                auto ins = *(acpt.begin());
                acpt.erase(acpt.begin());
                cells->push_back(ins);
            }
        }
        rests.erase(rests.begin());
        while (!rests.empty())
        {
            auto vt = *(rests.begin());
            rests.erase(rests.begin());
            for (auto itc = cells->begin(); itc != cells->end(); itc++)
            {
                auto c = *itc;
                if (IsVertexInCell(c, vt))
                {
                    cell_t cAB = { vt, c.A, c.B };
                    cAB.Sort();
                    cell_t cBC = { vt, c.B, c.C };
                    cBC.Sort();
                    cell_t cCA = { vt, c.C, c.A };
                    cCA.Sort();
                    standby.push_back(cAB);
                    standby.push_back(cBC);
                    standby.push_back(cCA);
                    cells->erase(itc);
                    break;
                }
            }
            cvec_t acpt;
            while (!standby.empty())
            {
                auto ins = *(standby.begin());
                standby.erase(standby.begin());
                bool engage = true;
                for (auto itc = cells->begin(); itc != cells->end(); itc++)
                {
                    auto old = *itc;
                    if (ins == old)
                    {
                        engage = false;
                        break;
                    }
                    coord_t va, vb, o1, o2;
                    bool common = FindCommon(va, vb, o1, o2, ins, old);
                    if (common)
                    {
                        bool inCircumcircle = IsVertexInCircumcircle(old.A, old.B, old.C, o1);
                        if (inCircumcircle)
                        {
                            cell_t c1 = { o1, o2, va };
                            c1.Sort();
                            cell_t c2 = { o1, o2, vb };
                            c2.Sort();
                            standby.push_back(c1);
                            standby.push_back(c2);
                            cells->erase(itc);
                            engage = false;
                            break;
                        }
                    }
                }
                if (engage)
                    acpt.push_back(ins);
            }
            while (!acpt.empty())
            {
                auto ins = *(acpt.begin());
                acpt.erase(acpt.begin());
                cells->push_back(ins);
            }
        }
    }
    else
    {
        rests.push_back(vtsConvexHull->back());
        vtsConvexHull->pop_back();
        noInnerVertices = true;
        goto TAG_SPLIT;
    }
    for (auto it = cells->begin(); it != cells->end();)
    {
        double radius = 0;
        auto cnt = FindCircumCircle(radius, (*it).A, (*it).B, (*it).C);
        if (cnt.first < 0 || cnt.first >= width || cnt.second < 0 || cnt.second >= height || radius <= 0)
            it = cells->erase(it);
        else
            it++;
    }
    for (auto p : *cells)
    {
        MKTriangle* tri = MKTriangle::Create();
        for (MKVertex* q : *vertices)
            if (q->x == p.A.first && q->y == p.A.second)
            {
                tri->A = q;
                break;
            }
        for (MKVertex* q : *vertices)
            if (q->x == p.B.first && q->y == p.B.second)
            {
                tri->B = q;
                break;
            }
        for (MKVertex* q : *vertices)
            if (q->x == p.C.first && q->y == p.C.second)
            {
                tri->C = q;
                break;
            }
        tri->Sort();
        bool found = false;
        for (MKTriangle* q : *triangles)
            if (*q == *tri)
            {
                found = true;
                break;
            }
        if (!found)
            triangles->push_back(tri);
    }
    delete vtsConvexHull;
    vtsConvexHull = nullptr;
    delete cells;
    cells = nullptr;
}

void MapKit::ConvexHull(vtvec_t*& res, const vtvec_t* _vts)
{
    res = new vtvec_t;
    double minY = double(0x7FFFFFFF);
    auto minIt = _vts->end();
    for (auto it = _vts->begin(); it != _vts->end(); it++)
        if ((*it).second < minY)
        {
            minY = (*it).second;
            minIt = it;
        }
    auto baseIt = minIt;
    auto searchIt = _vts->begin();
    if (minIt == _vts->end())
        searchIt++;
    do
    {
        double dot = 1.1;
        for (auto it = _vts->begin(); it != _vts->end(); it++)
            if (it != searchIt && it != baseIt)
            {
                glm::dvec3 base((*searchIt).first - (*baseIt).first, (*searchIt).second - (*baseIt).second, 0);
                glm::dvec3 n((*it).first - (*baseIt).first, (*it).second - (*baseIt).second, 0);
                auto c = glm::cross(base, n);
                if (c.z <= 0)
                {
                    searchIt = it;
                    dot = c.z;
                }
            }
        baseIt = searchIt;
        res->push_back(*searchIt);
        for (auto it = _vts->begin(); it != _vts->end(); it++)
        {
            bool found = true;
            for (auto p : *res)
                if (p == *it)
                {
                    found = false;
                    break;
                }
            if (found)
            {
                searchIt = it;
                break;
            }
        }
    } while (baseIt != minIt);
}

bool MapKit::FindCommon(coord_t& va, coord_t& vb, coord_t& opposite1, coord_t& opposite2, const cell_t& t1, const cell_t& t2)
{
    // AB - AB
    if ((t1.A == t2.A && t1.B == t2.B) || (t1.A == t2.B && t1.B == t2.A))
    {
        va = t1.A;
        vb = t1.B;
        opposite1 = t1.C;
        opposite2 = t2.C;
        return true;
    }
    // AB - BC
    if ((t1.A == t2.B && t1.B == t2.C) || (t1.A == t2.C && t1.B == t2.B))
    {
        va = t1.A;
        vb = t1.B;
        opposite1 = t1.C;
        opposite2 = t2.A;
        return true;
    }
    // AB - CA
    if ((t1.A == t2.C && t1.B == t2.A) || (t1.A == t2.A && t1.B == t2.C))
    {
        va = t1.A;
        vb = t1.B;
        opposite1 = t1.C;
        opposite2 = t2.B;
        return true;
    }
    // BC - AB
    if ((t1.B == t2.A && t1.C == t2.B) || (t1.B == t2.B && t1.C == t2.A))
    {
        va = t1.B;
        vb = t1.C;
        opposite1 = t1.A;
        opposite2 = t2.C;
        return true;
    }
    // BC - BC
    if ((t1.B == t2.B && t1.C == t2.C) || (t1.B == t2.C && t1.C == t2.B))
    {
        va = t1.B;
        vb = t1.C;
        opposite1 = t1.A;
        opposite2 = t2.A;
        return true;
    }
    // BC - CA
    if ((t1.B == t2.C && t1.C == t2.A) || (t1.B == t2.A && t1.C == t2.C))
    {
        va = t1.B;
        vb = t1.C;
        opposite1 = t1.A;
        opposite2 = t2.B;
        return true;
    }
    // CA - AB
    if ((t1.C == t2.A && t1.A == t2.B) || (t1.C == t2.B && t1.A == t2.A))
    {
        va = t1.C;
        vb = t1.A;
        opposite1 = t1.B;
        opposite2 = t2.C;
        return true;
    }
    // CA - BC
    if ((t1.C == t2.B && t1.A == t2.C) || (t1.C == t2.C && t1.A == t2.B))
    {
        va = t1.C;
        vb = t1.A;
        opposite1 = t1.B;
        opposite2 = t2.A;
        return true;
    }
    // CA - CA
    if ((t1.C == t2.C && t1.A == t2.A) || (t1.C == t2.A && t1.A == t2.C))
    {
        va = t1.C;
        vb = t1.A;
        opposite1 = t1.B;
        opposite2 = t2.B;
        return true;
    }
    return false;
}

bool MapKit::IsVertexInCircumcircle(const coord_t& A, const coord_t& B, const coord_t& C, const coord_t& P)
{
    double x1 = A.first, y1 = A.second;
    double x2 = B.first, y2 = B.second;
    double x3 = C.first, y3 = C.second;
    glm::dvec2 mid2 = { (x2 + x1) / 2, (y2 + y1) / 2 };
    glm::dvec2 mid3 = { (x3 + x1) / 2, (y3 + y1) / 2 };
    glm::dmat2 mm2 = {
        { x2 - x1, -(y2 - y1) },
        { mid2.y, mid2.x }
    };
    glm::dmat2 mm3 = {
        { x3 - x1, -(y3 - y1) },
        { mid3.y, mid3.x }
    };
    double detm2 = glm::determinant(mm2);
    double detm3 = glm::determinant(mm3);
    glm::dvec2 eqVec = { detm2, detm3 };
    glm::dmat2 eqMat = {
        { x2 - x1, y2 - y1 },
        { x3 - x1, y3 - y1 }
    };
    auto eqMatInv = glm::inverse(eqMat);
    auto center = glm::transpose(eqMatInv) * eqVec;
    auto radius = glm::distance(center, { x1, y1 });
    auto rd = glm::distance(center, { P.first, P.second });
    return rd < radius;
}

bool MapKit::IsVertexInCell(const cell_t& c, const coord_t& vt)
{
    double x1 = c.A.first, y1 = c.A.second;
    double x2 = c.B.first, y2 = c.B.second;
    double x3 = c.C.first, y3 = c.C.second;
    glm::dvec2 t = { vt.first - x1, vt.second - y1 };
    glm::dvec2 b2 = { x2 - x1, y2 - y1 };
    glm::dvec2 b3 = { x3 - x1, y3 - y1 };
    glm::dmat2 eqMatAB = {
        { b2.x, b3.x },
        { b2.y, b3.y }
    };
    glm::dvec2 eqVecAB = { t.x, t.y };
    auto eqMatABInv = glm::inverse(eqMatAB);
    auto ab = glm::transpose(eqMatABInv) * eqVecAB;
    glm::dmat2 eqMatLG = {
        { t.x, b2.x - b3.x },
        { t.y, b2.y - b3.y }
    };
    glm::dvec2 eqVecLG = { b2.x, b2.y };
    auto eqMatLGInv = glm::inverse(eqMatLG);
    auto lg = glm::transpose(eqMatLGInv) * eqVecLG;
    return ab.x >= 0 && ab.y >= 0 && lg.x >= 1 && ab.x * ab.y > 0;
}

MapKit::coord_t MapKit::FindCircumCircle(double& radius, const coord_t& A, const coord_t& B, const coord_t& C)
{
    double x1 = A.first, y1 = A.second;
    double x2 = B.first, y2 = B.second;
    double x3 = C.first, y3 = C.second;
    glm::dvec2 mid2 = { (x2 + x1) / 2, (y2 + y1) / 2 };
    glm::dvec2 mid3 = { (x3 + x1) / 2, (y3 + y1) / 2 };
    glm::dmat2 mm2 = {
        { x2 - x1, -(y2 - y1) },
        { mid2.y, mid2.x }
    };
    glm::dmat2 mm3 = {
        { x3 - x1, -(y3 - y1) },
        { mid3.y, mid3.x }
    };
    double detm2 = glm::determinant(mm2);
    double detm3 = glm::determinant(mm3);
    glm::dvec2 eqVec = { detm2, detm3 };
    glm::dmat2 eqMat = {
        { x2 - x1, y2 - y1 },
        { x3 - x1, y3 - y1 }
    };
    auto eqMatInv = glm::inverse(eqMat);
    auto center = glm::transpose(eqMatInv) * eqVec;
    radius = glm::distance(center, { x1, y1 });
    return { center.x, center.y };
}